#include <vector>
#include <string>

#include "matplotlibcpp.hpp"

// header for map
#include "modules/common/configs/config_gflags.h"
#include "modules/map/proto/map.pb.h"
#include "modules/common/util/string_util.h"
#include "modules/map/hdmap/adapter/opendrive_adapter.h"
#include "modules/map/hdmap/hdmap_impl.h"

// headers for routing
// #include "modules/routing/routing_component.h"

#include <utility>
#include "modules/routing/routing_component.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/routing/common/routing_gflags.h"
#include <memory>

#include "modules/routing/routing.h"
#include "cyber/cyber.h"

DEFINE_bool(enable_remove_lane_id, true,
            "True to remove lane id in routing request");

namespace apollo
{
  namespace routing
  {

    bool RoutingComponent::Init()
    {
      apollo::cyber::proto::RoleAttributes attr;
      attr.set_channel_name(FLAGS_routing_response_topic);
      auto qos = attr.mutable_qos_profile();
      qos->set_history(apollo::cyber::proto::QosHistoryPolicy::HISTORY_KEEP_LAST);
      qos->set_reliability(
          apollo::cyber::proto::QosReliabilityPolicy::RELIABILITY_RELIABLE);
      qos->set_durability(
          apollo::cyber::proto::QosDurabilityPolicy::DURABILITY_TRANSIENT_LOCAL);
      response_writer_ = node_->CreateWriter<RoutingResponse>(attr);

      apollo::cyber::proto::RoleAttributes attr_history;
      attr_history.set_channel_name(FLAGS_routing_response_history_topic);
      auto qos_history = attr_history.mutable_qos_profile();
      qos_history->set_history(
          apollo::cyber::proto::QosHistoryPolicy::HISTORY_KEEP_LAST);
      qos_history->set_reliability(
          apollo::cyber::proto::QosReliabilityPolicy::RELIABILITY_RELIABLE);
      qos_history->set_durability(
          apollo::cyber::proto::QosDurabilityPolicy::DURABILITY_TRANSIENT_LOCAL);

      response_history_writer_ = node_->CreateWriter<RoutingResponse>(attr_history);
      std::weak_ptr<RoutingComponent> self =
          std::dynamic_pointer_cast<RoutingComponent>(shared_from_this());
      timer_.reset(new ::apollo::cyber::Timer(
          FLAGS_routing_response_history_interval_ms,
          [self, this]() {
            auto ptr = self.lock();
            if (ptr)
            {
              std::lock_guard<std::mutex> guard(this->mutex_);
              if (this->response_.get() != nullptr)
              {
                auto response = *response_;
                auto timestamp = apollo::common::time::Clock::NowInSeconds();
                response.mutable_header()->set_timestamp_sec(timestamp);
                this->response_history_writer_->Write(response);
              }
            }
          },
          false));
      timer_->Start();

      return routing_.Init().ok() && routing_.Start().ok();
    }

    bool RoutingComponent::Proc(const std::shared_ptr<RoutingRequest> &request)
    {
      auto response = std::make_shared<RoutingResponse>();
      if (!routing_.Process(request, response.get()))
      {
        return false;
      }
      common::util::FillHeader(node_->Name(), response.get());
      response_writer_->Write(response);
      {
        std::lock_guard<std::mutex> guard(mutex_);
        response_ = std::move(response);
      }
      return true;
    }

  } // namespace routing
} // namespace apollo

/*
export CYBER_PATH=../../apollo/cyber/
export GLOG_log_dir=~

export CYBER_DOMAIN_ID=80
export CYBER_IP=127.0.0.1
export GLOG_alsologtostderr=0
export GLOG_colorlogtostderr=1
export GLOG_minloglevel=0
export cyber_trans_perf=0
export cyber_sched_perf=0
*/

int main(int argc, char *argv[])
{

  // Load the routing request file
  // routing request waypoints
  constexpr char routing_test_waypoints[] = "../src/routing_test_waypoints.txt";
  apollo::routing::RoutingRequest routing_request;
  if (!apollo::cyber::common::GetProtoFromFile(routing_test_waypoints,
                                               &routing_request))
  {
    AERROR << "failed to load file: " << routing_test_waypoints;
    return -1;
  }
  else
  {
    AERROR << "Load Routing request file success";
  }
  AERROR << "&routing_request   " << &routing_request;

  apollo::routing::RoutingRequest *routing_request_1 = new apollo::routing::RoutingRequest(routing_request);
  std::shared_ptr<apollo::routing::RoutingRequest> routing_request_2(routing_request_1);

  // generate topo_graph
  // modules/common/data/global_flagfile.txt
  apollo::routing::Routing routing_;
  routing_.Init();
  routing_.Start();

  // routing process
  auto response = std::make_shared<apollo::routing::RoutingResponse>();
  routing_.Process(routing_request_2, response.get());
  // 打印routing的结果
  AERROR << "routing results:     " << (response->status().msg());
  response->road().size();

  for (int i = 0; i < response->road().size(); i++)
  {
    std::cout << "  " << response->road()[i].id() << std::endl;
  }

  response->measurement();
  response->routing_request();
  response->map_version();
  response->status();

  ///////////////////

  AERROR << " routing demo";
  return 0;
}